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OPTIMAL  CONTROL  AND  FILTER  GAINS  FOR  THE  STATIONARY  CONTINUOUS 
OR  DISCRETE  TIME  LQG  PROBLEM  -  A  FORTRAN  PROGRAM 

INTRODUCTION 

In  this  report  the  solution  to  the  continuous  and  discrete-time  linear- 
quadratic  regulator  (LQR)  and  Kalman  filter  (KF)  is  presented,  and  a  FORTRAN 
program  is  included.  The  LQR  or  KF  design  model  for  a  stationary  process  is 
described  as  a  continuous  or  discrete  vector-matrix  equation.  The  output  is 
the  control  system  or  filter  eigenstructure  and  the  optimal  steady  state  LOR 
or  KF  gain  matrices.  The  method  used  to  compute  the  gains  is  the  classical 
eigenvalue-eigenvector  approach. 

The  common  requirement  in  the  design  of  an  optimal  LOR  control  law  or  a 
KF  is  the  solution  to  the  control  or  filter  Riccati  equation  [1],  In  the 
general  case,  the  solution  is  obtained  by  a  backwards  in  time  propagation  from 
a  known  tei  minal  boundary  for  the  LQR,  and  by  a  forward  in  time  propagation 
for  the  KF. 

In  most  practical  design  problems,  it  is  assumed  that  the  control  or 
filter  design  model  is  time-invariant.  With  this  assertion,  the  Riccati 
solution  reaches  a  steady  state  value  in  a  few  sytem  time  constants.  The 
steady  state  solution  to  the  Riccati  equation  is  subsequently  used  to  compute 
the  steady  state  LQR  or  KF  gains,  which  are  used  to  implement  the  candidate 
LQR  or  KF.  In  many  cases,  a  KF  is  used  to  estimate  the  state  vector  in  the 
LQR  control  law.  This  latter  strategy  is  referred  to  as  a  linear-quadratic- 
gausslan  (LQG)  control  design  f2]. 
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The  soluton  to  the  Riccati  equation  by  integration  requires  the  solution 
of  nx  x  (n^j  +  l)/2  coupled  differential  equations,  where  nx  is  the  dimension 
of  the  plant  model.  When  only  the  steady-state  solution  is  required,  an 
alternative  is  to  solve  the  algebraic  version  of  the  Riccati  equation,  i.e., 
the  derivative  elements  are  set  at  zero.  This  results  in  a  set  of 
simultaneous  equations,  which  must  be  solved  by  computer  for  plant  models  of 
greater  than  2nd-order. 

There  are  several  possible  methods  of  solving  the  algebraic  Riccati 
equation.  The  method  used  in  this  effort  is  the  classical  eigenvalue- 
eigenvector  approach,  which  was  first  described  by  HacFarlane  [3]  and  Potter 
[4J»  The  MacFarlane-Potter  method,  which  originally  was  applied  to 
continuous-time  plant  models,  was  extended  to  discrete-time  plant 
representations  by  Vaughn  [5].  This  latter  development  enables  us  to  use  the 
same  basic  approach  to  design  a  LOR  or  KF  for  a  discrete  or  continuous-time 
design  model.  Another  advantage  of  this  approach  is  that  the  eigenstructure 
of  the  closed  loop  LQR  or  KF  is  a  by-product  of  the  Riccati  equation 
solution.  These  results  are  an  invaluable  aid  in  evaluating  the  candidate 
designs.  This  is  particularly  true  in  the  scaler  input  cases,  where  the 
position  of  dosed  loop  eigenvalues  are  a  direct  Indication  of  the  control  or 
filter  transient  response. 

The  selected  method  requires  the  computation  of  the  eigenvalues  and 
eigenvectors  of  the  appropriate  Hamiltonian  matrix.  This  difficult 
computation  is  facilitated  with  the  use  of  a  few  subroutines  from  EISPACK 
(6].  EISPACK  is  a  software  package  which  is  the  product  of  an  Intensive 
effort  to  develop  reliable  methods  of  computing  the  eigenstructure  of  various 
matrix  types.  The  appropriate  Hamiltonian  matrix  is  easy  to  set  up  from  a 
common  set  of  input  matrices  for  all  design  model  and  problem  options 


described 


Notation 

The  following  notation  is  used  throughout  this  report: 

Underlined  uncapitalized  letters  will  denote  column  vectors,  with  the 
dimension  indicated  by,  for  example,  nx  for  the  vector  £.  In  the  continuous 
time  case,  vectors  are  an  implied  function  of  time.  In  the  discrete  time  case 
the  notation  (k),  (k  +  1),  ...  will  denote  the  vector  at  discrete  times  t^, 
tfc+i*  ...»  where  k  -  0,  1,  ...  n.  T  denotes  the  constant  time  interval 
tfc+j  -  tfc.  The  superscript  denotes  the  transpose  of  a  matrix  or  vector,  and 
*  and  *  over  a  vector  denotes  the  time  derivative  and  estimate  of, 
respectively. 

A  standard  set  of  matrix  symbols,  which  are  denoted  by  capital  letters, 
are  used  to  define  the  plant  models  and  design  parameters  for  all  options. 

This  is  done  to  simplify  the  input  data  required.  The  subscripts  c  and  £  in 
the  equations  indicates  whether  the  particular  matrix  is  associated  with  a  LOR 
or  IT.  A  “  over  the  matrix  denotes  the  discrete-time  equivalent  of  a  matrix 
in  the  continuous-time  model,  i.e.,  A. 

The  symbol  E  is  the  expectation  operator,  (i.e.,  the  average  value  of), 
and  s  is  the  Laplace  transform  operator. 

PROBLEM  DEFINITION  AND  SOLUTION 
A.  Continuous -Time  LQR  Problem 

The  design  model  for  the  plant  to  be  controlled  is 

x-Ax  +  Bu  (la) 

£  -  C  £  (lb) 

*****  £»  JL»  —  *r<  th*  state,  control  input,  and  output  vectors  of 
dimensions  nx,  tig,  and  nc  x  1,  respectively.  A,  B,  and  C  are  constant  real 
plant,  control  input  (distribution),  and  output  matrices,  respectively.  The 


control  law  to  be  used  Is  of  the  linear  state  variable  feedback  (LSVF)  form 


u_  ■  Gc  x_  (lc) 

where  Gc  is  a  constant  nu  x  nx  matrix  of  control  gains  (to  be  selected).  The 
LQR  design  procedure,  which  results  in  a  control  law  in  the  form  of  (lc)  is  to 
be  used  to  select  an  optimal  gain  matrix  in  the  linear  quadratic  (LO)  sense. 

We  note  that  there  are  various  other  design  procedures  for  selecting  non- 
optimal  gains,  l.e.,  pole -placement,  classical  frequency  domain  methods,  etc. 

In  the  LQR  design  procedure  used  here,  a  quadratic  cost  function  of  one 
of  the  following  forms  is  selected. 

(a)  state  regulator  cost 

2  ^  -  ro  <*T  Qc  *  +  “T  Rc  u)dt  (Id) 

(b)  output  regulator  cost 

2  Jfe  "  (£T  cT  Qc  c  £  +  uT  Rc  u)dt  (le) 

and  R^.  in  (Id,  le)  are  symmetric,  real  state  and  control  input 
weighting  matrices,  respectively,  with  Qc  restricted  to  be  positive  semi- 
definite  (>  0)  and  Rc  restricted  to  be  positive  definite  (>  0).  The  optimal 
gain  which  minimizes  Jj  or  J2  is 

Gc  -  -  R"1  Bt  K  (If) 

where  K  is  a  constant  matrix  (>  0)  in  the  time  invariant  case.  K  is  obtained 
by  solving  the  algebraic  control  Riccatl  equation 

AT  K  +  K  A  +  Q*  -  K  B  R_I  BT  K  -  0  (lg) 

c  c 

f 

where  Q£  -  Qcfor  the  state  regulator,  and 

•  T 

Q  ■  C*  Q  C  for  the  output  regulator 
Remark  on  the  constant  LQ  gain  strategy: 

An  alternate  method  of  computing  K  is  to  solve  the  differential  control 
Uccati  equation 

V(t)  -  AT  K(t)  +  K(t)  A  +  0^  -  K(t)  B  R;1  BT  K(t)  (lh) 
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backwards  in  time  from  K(°°  )  *  0.  K(t)  converges  to  K  in  a  few  system  time 
constants.  The  use  of  K  to  compute  a  constant  optimal  gain  is  referred  to  as 
the  infinite-time  LQR  in  the  literature,  cf  [7,  Chap.  9).  If  A,  B,  C,  and  0C 
and  (or)  R^.  are  not  constant,  the  solution  of  K(t)  is  indicated.  However,  the 
time  histories  of  these  matrices  are  rarely  known  a-prior.  Hence  the  strategy 
in  the  time-varying  case  is  to  select  a  set  of  stationary  plant  models 
corresponding  to  expected  operating  points,  i.e.,  various  equilibrium 
conditions.  The  corresponding  set  of  LOR  gains  are  computed  and  scheduled  as 
a  function  of  some  convenient  measured  variable,  i.e.,  dynamic  pressure  in  an 
aircraft.  An  alternative  is  to  select  a  constant  gain  matrix  which  satisfies 
the  set  of  plant  models  (i.e.,  by  simulation  studies).  See  f7,  Chap.  9]  for 
an  excellent  discussion  on  the  impracticalness  of  Implementing  Gc(t). 

B.  Algebraic  Riccati  Equation  Solution 


The  Hamiltonian  matrix  associated  with  the  LQR  problem  [3]  is 


•  T 

Qc  a 


where  S  -  B  R-1  BT 
c 

Hc,  which  it  a  2  Dj  x  2  iij  matrix,  has  2  nx  eigenvalues.  For  each  eigenvalue 
X,  which  appear  in  conjugate  pairs  if  complex,  -  X  is  also  an  eigenvalue. 

Those  eigenvalues  with  negative  real  parts  are  the  closed  loop  eigenvalues  of 
the  LQR,  that  is,  they  are  poles  of  the  system  characteristic  equation 

|sl  -  (A  +  B  Gc)|  -  0  (1.1) 

We  note  that  the  LQR  design  procedure  guarantees  a  stable  control  law,  with 
certain  gain  and  phase  margins.  If  the  plant  is  controllable  [9]  f 10 ] . 


•a  -V  •  •  *  */ 


■ipjivipi* 


V  ?  TT? M  VI  V-  ■->  ?  V  'A  V- «3  \  V «71 


Define  the  block  matrix  of  eigenvectors  associated  with  H£ 


W 


Wn 

W21 

w2i 

W22 

(lk) 


W, 


Where 


11 

W21 


contains  the  upper  and  lower  half  elements  of  the  eigenvectors  associated 
with  the  eigenvalues  of  Hc  with  positive  real  parts.  The  eigenvectors  can  be 
arranged  in  any  order,  except  that  those  due  to  complex  conjugate  eigenvalues 
are  adjacent.  The  solution  to  the  algebraic  Riccatl  equation  is  [3,  4] 

K  Wn  -  Wa  (11) 

Instead  of  solving  for  K  by  computing  the  inverse  of  Vfy  j  ,  it  is  best  to 
manipulate  (2d)  into  the  linear  system  form 

Wn  K  -  (lm) 

K  is  computed  with  the  use  of  any  a  linear  system  solver.  In  this  effort  we 
use  the  method  described  in  [11]. 

Remark  on  the  Hamiltonian: 

The  Hamiltonian  matrix  is  associated  with  the  Euler-Lagrange  system  of 
linear  equations  [7] 


m 

• 

X 

X 

• 

"  Hc 

£ 

•  m 

£ 

where  £  Is  the  nx  x  1  costate  vector.  The  solution  of  the  above  set  is  a  2- 
polnt  boundary  value  problem  with  jc(0)  and  £(•)  known.  The  solution  of  the 

State  equation  which  minimize  Ji  or  Jfc  is 

•  “*1  T 

x  ■  Ax  -  B  R  1  B1  d 
—  c 

where  £  ■  K(t)  x. 


C.  Discrete-Time  LOR  Problem 


In  the  discrete-time  case, the  (normally  continuous)  plant  design  model  is 
described  in  the  form 

jc  (k+1)  ■  A  x  (k)  +  B  jj  (k)  (2a) 

£  (k)  -  C  x  (k)  (2b) 

where  A  and  B  are  discrete-time  equivalents  of  A  and  B  in  (la),  and  u^  (k)  is 
assumed  to  be  piecewise  continuous  in  the  constant  time  interval 


T  *  ttxi  —  tu,  k  *  0,1, ••• 


tk+l  '■k 

A  and  B  are  given  by 


A  -  exp  (AT) 


B  -  J  *  A  (T,  t)B  dt 


(2c) 

(2d) 


Since  (lb)  and  (2b)  are  algebraic,  C  does  not  change. 

tfe  have  used  various  methods,  which  are  not  presently  included  in  the 
software  described  here,  to  compute  A  and  B.  The  potential  methods  and 
possible  problems  are  an  interesting  subject,  c.f.  [8).  If  T  is  selected  to 
be  sufficiently  small,  in  comparison  with  the  plant  time  constants,  the 
following  first  order  approximation  are  useful: 

A-  I  +  AT  (2e) 

B  -  BT  (2f ) 

The  LQR  design  procedure  [12]  for  the  discrete  plant  is  outlined  below: 

LQ  Control  Law: 

(2g) 


u(k)  -  G  x(k) 

C  11,1 


where  6  is  a  constant  LQ  gain  matrix 


Quadratic  Cost  Functions: 

state  regulator 


r  T„. 


'  R  u(k)] 


(2h) 


output  regulator 

00 

2  ;  -1  [  z_T(k)  CT  o  C  z ( k)  +  uT(k)  R  u(k)] 

-  .  k-o  *”  c 

where  0  and  R  are  the  discrete  tine  equivalents  of  CL  and  R„. 
c  c  c  c 

LOR  gain: 

G  -  -  [  R  +  BT  K  BT]  BT  K  A 
c  L  c  J 

where  K  is  the  solution  to  the  discrete  algebraic  Riccati  equation, 
Discrete  algerbraic  Riccati  equation: 

at  k  a  -atkb[  bt  kb  +  r  ]  -i  bt  k  a  +  V  -  K  -  0 

1  cJ  c 

*  i 

where  Q£  ■  Qc  (state  regulator) 

T  ~ 

or  C  Q^C  (output  regulator) 

Hamiltonian : 


A'1 

a”1  s 

~  ~  -1 

Qc  A 

at  +  o 

< 

’  r1 

c 


where  S  -  B  R_1  BT 


Hc  has  2  eigenvalues  with  the  following  property:  For  each  eigenvalue 
A  (assumed  to  be  within  the  unit  circle),  X  *  is  also  an  eigenvalue  (outside 
the  unit  circle).  Complex  eigenvalues  appear  in  conlugate  nairs.  The 
eigenvalues  within  the  unit  circle  of  the  z-plane  are  the  closed  loop  poles  of 
the  discrete  LQR  characteristic  equation. 

Discrete  Riccati  equation  solution: 

The  method  follows  that  of  Vaughn  [5]. 


Let  I  -  I  be  a  partitioned  matrix  of  eigenvectors  corresponding  to 

L  L^iJ 

those  eigenvectors  outside  the  unit  circle,  l.e.,  the  unstable  poles.  An 
eigenvalue  X  is  outside  the  unitcircle  if  X  ,  or  its  vector  sum,  is  >  1.0.  The 
organization  of  the  eigenvectors  is  as  in  the  continuous  time  case.  The 


Aj  <  V.V  .‘a*.  ■ , ’ •  a!  a’  il'W  i"* .• 
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a 


solution  to  (2k)  is 

'  t  ~  ~  t 

Wll  K  -  »>1 

Where  K  is  solved  as  in  the  continuous -time  case. 

D.  Kalman  Filter  Problem 
Continuous  Time  Filter 

In  the  KF  problem  the  objective  is  to  estimate  the  state  of  the  linear 
stochastic  plant 

x«Ax+Bu+w  (3a) 

z_-  C  x.  +  v_  (3b) 

where  x,  u,  z.  A,  B,  and  C  are  as  previously  defined,  and  w_  and  v_  are 

independent,  zero-mean,  gaussian  white  plant  and  output  (measurement)  noise 

processes,  respectively.  The  intensity  matrices  (spectral  densities)  of  and 

v_  are  Qf  (>  0)  and  Rf  (>  0). 

The  Kalman  filter  for  this  system  (c.f.  [1]  is 
* 

x»Ax  +  Bu  +  G{  [.z  -  C  x]  (3c) 

a 

Where  x  is  the  optimal  estimate  of  x.  and  Gf  is  a  nx  x  nz  matrix  of  constant 

Kalman  gains.  The  state  error  covariance  matrix  P  is  defined  as 

P  -  E  {fijc  5xT} 

* 

where  fix  ■  x  -  x 

The  initial  state  error  covariance  P  (t-0)  -  PQ  is  assumed  to  be  known. 

The  Kalman  gain  matrix  is  given  by 

Gf  -  P  CT  R"1  (3d) 

Where  the  symmetric  matrix  P,  >  0,  is  obtained  by  solving  the  algebraic 
covariance  Rlccatl  equation 

AP  +  PAT  +  Qj  -  P  CT  Rj1  C  P  -  0  (3e) 

Mote  that  the  dimension  of  Gc  and  Gf  are  different. 

The  KF  can  be  viewed  as  a  closed  loop  control  sytem.  The  eigenvalues  of 


Che  KF  are  Che  roocs  of 


| si  -  A  +  Gf  C|  -  0 
Covariance  RiccaCi  equaCion  soluCion: 

The  HamilConian  maCrix  for  Che  KF  is 


Where  Hf  is  2^x2  n^.  Hf  has  Che  same  properCies  as  Che  conCrol 
HamilConian  Hc,  Vaughn  [5].  Hence  Che  sCeady  scaCe  covariance  macrlx  is 
obcained  from 

T  T  * 

«fl  P  -  « 21  ( 3g) 

where  Che  parCiCioned  maCrix 

conCains  Che  eigenvecCors  of  Hf  associated  wich  Che  positive  eigenvalues. 
Comment  on  Che  time  varying  Kalman  gain: 

The  time  varying  state  error  covariance  matrix  P(C)  is  obtained  by 

integrating  Che  differential  RiccaCi  equation 

*  -1 
P(t)  -  A  P  (t)  +  P(t)  A  +  0f  -  P(t)  CT  Rf  C  P(t) 

forward  in  time  from  PQ.  Hence  it  is  feasible  Co  compute  Gf(t)  in  real 
time.  The  only  advantage  to  this  approach  in  the  case  of  a  stationary  olant 

A 

is  that  it  will  converge  to  x_  more  quickly.  In  the  case  of  a  time-varying 
plant,  it  is  normal  to  implement  the  time  varying  Kalman  filter. 

Discrete  time  Kalman  filter: 

Since  the  continuous  KF  equation  (3c)  contains  the  deterministic  portion 
of  the  plant  model,  it  is  quite  complex  to  Implement  with  analog  circuitry. 
This  indicates  the  use  of  a  digital  computer,  and  the  discrete  version  of  the 
KF.  For  this  reason,  (3c)  is  seldom  used.  The  discrete  version  of  the 
continuous  stochastic  plant  model  is 


(4a) 


x(k+l)  -  A  jc(k)  +  B  u(k)  +  ^(k) 

:(k)  •  C  x(k)  +  v(k)  (4b) 

Where  w(k),  ^(k)  are  discrete  white  noise  sequences  representing  plant 
and  measurement  noise,  andQ^  and  are  the  covariances  of  w(k)  and  v(k), 
respectively.  There  are  two  common  forms  of  the  KF  in  general  use.  These  are 
(i)  the  filter  form,  where 

A 

x(k+l)  -  E  { jc(k+l)  |  z(o ) ,  ....  z  (k+1)} 
and  (ii)  the  one  step  ahead  predictor  form  where 
x(k+l)  -  E  {jt(k+l)  |  z  (o)  ,...,  z  (k)} 

However  both  forms  are  referred  to  as  a  "filter"  in  the  literature. 

(i)  Filter  algorithm 

The  filtered  jk  is  normally  computed  in  time  and  measurement  update 
*  —  *  + 

stages,  where  jc  (k)  and  x  (k)  will  denote  the  time  and  measurement  updated 
state  estimates 

Time  update 

x  (k+1)  -  Ax'(k)  +  B  u(k)  (4c) 

Measurement  update 

x  +(k+l)  -  x  "(k+1)  +  ~Gf  [  z  (k+1)  -  C  x  "(k+1)]  (4d) 

where  6^  is  a  matrix  of  constant  filter  gains. 

(ii)  Predictor  Algorithm 

The  one-step  ahead  predictor  algorithm  is 
x(k+l)  -  A  x(k)  +  B  u(k)  +  6  [  z(k)  -  C  x(k)]  (4e) 

where  is  a  constant  matrix  of  predictor  gains. 

Remarks: 

The  notation  (k+1 1 k) ,  (k|k-l),  etc  is  often  used  to  denote  the  one-step 
ahead  predicted  estimate  of  x.« 
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Optimal  Gains 


The  filter  and  predictor  gains  are  given  by 


P  CT  [  C  P  CT  +  R  ] 


-1 


(4f  ) 

and  G  *  A  G,  ( 4g ) 

P  f 

Where  P  is  the  steady-state,  discrete,  state  error  covariance,  and  is  obtained 
by  solving  the  discrete  algebraic  covariance  Riccati  equation 
A  P  AT  +  Qj  -  A  P  CT  [  C  P  CT  +  Rf]  _1  C  P  AT  -  P  -  0 
Riccati  Equation  Solution 

The  Hamiltonian  associated  with  the  discrete  KF  is 


(4h) 


Hf  “ 


.-T 


Qf  A 


-T 


~  -T  ~  -1 
A  Rf 

-  -  -  _t  ~  _i 

A  +  Qf  A  Rf 


Where  the  2  x  2  matrix  contains  2  nx  eigenvalues  with  the  same 
properties  as  the  discrete  LQR  Hamiltonian  Hc<  The  eigenvalues  A  of  Hf  which 
are  inside  the  unit  circle  are  the  closed  loop  poles  of  the  discrete  KF.  P  is 
computed  using  the  same  method  as  in  the  discrete  LQR  equation  (2k),  and  as 
first  described  in  [5]. 

FORTRAN  PROGRAM  DESCRIPTION 

This  section  describes  a  FORTRAN  progrm  which  computes  the  steady  state 
LQR  or  KF  gains,  as  given  in  the  previous  section.  A  program  listing  is  given 
in  Appendix  A.  Several  separate  programs,  which  have  been  used  by  this  author 
over  a  period  of  several  years,  were  combined  to  produce  the  version 
described.  Hence  the  result  is  not  as  optimal,  from  a  viewpoint  of  work 
vectors  and  matrices,  execution  time,  and  modularity,  as  the  program  could  be 
if  we  had  started  from  scratch.  Subroutines  were  used  only  to  eliminate 
obvious  duplications.  In  some  cases,  we  use  a  loop  instead  of  an  available 
subroutine,  if  only  2  or  3  statements  are  required. 


All  real  FORTRAN  variables  or  constants  are  in  double  precision,  as 


defined  in  the  IMPLICIT  statement.  This  allows  for  easy  change  from  double  to 
single  precision  computations  (with  appropriate  subroutine  changes).  In 
general,  matrix  and  vector  names  end  in  M  and  V,  respectively.  The  exceptions 
are  the  real  vectors  TV1,  TV2,  and  integer  vectors  IV1,  IV2.  All  integer 
variables  and  constants  begin  with  I  and  loop  counters  begin  with  I  or  J. 

The  maximum  dimension  of  the  state  vector  is  set  to  15  in  the  listed 
version.  This  limit  can  be  expanded  by  changing  the  integer  INXMX  to  the 
desired  value,  and  by  expanding  the  matrix  and  vector  dimensions 
accordingly.  All  subroutines  use  variables  dimensions. 

Data  Input 

The  FORTRAN  input  file  (FT05F001)  for  each  run  consists  of  the  following 
types  of  data  cards: 

(a)  Control/title  card  (No.  1) 

The  Integers  in  the  first  five  columns  are  used  to  set  the  status  of 
the  five  internal  option  control  flags  shown  in  Table  1.  The  characters  in 
columns  6  through  65  are  used  for  run  identification.  The  format  of  this  card 
is  (511,  15A4).  Note  that  the  flag  IDATAF  is  used  to  terminate  the  run  (*  0), 
i.e.,  a  blank  card. 

(b)  Plant  dimension  card  (No.  2) 

The  dimensions  of  x,  ^1*  and  *_  are  input  in  columns  1-2,  3-4,  and 
5-6,  respectively  (FORMAT  ■  312).  These  Integers  determine  the  dimensions  of 
the  matrices  to  follow. 

(c)Matrix  data  cards 

Each  matrix  to  be  read  in  is  preceded  by  a  card  containing  a  read 
(l)/no-read  (0)  matrix  flag  in  column  1.  We  use  columns  6  on  to  identify  the 
matrix;  however,  this  is  not  printed  out.  The  coefficients  of  the  matrix  to 
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Table  1  -  Control/Title  Card  Option  Flags 


Flag 

Card  Column 

Option 

IDATAF 

1 

1  “  run,  0  ■  stop 

IOREGF 

.  ,2 

0  ■  state  regulator,  1  -  output 
regulator  (LOR  option  only) 

EDISCF 

3 

0  •  continuous  plant 

1  *  discrete  plant  model 

IPRTF 

4 

0  •  normal  print 

1  *  extended  print  option 

ITYPE 

5 

0  *  LOR  option 

1  «  Kalman  filter  option 

be  input  are  read  in  by  rows  on  cards  following  the  read/no-read  flag  card. 

The  matrix  coefficient  card  format  is  6D12.8.  The  order  and  dimensions  of  the 


matrices  are: 

A  (nx  x  nx),  B  (nx  x  nu),  C  (nz  x  i^),  0  (nx  x  nx),  and  R,  which  is 
nu  x  iiy  for  the  LOR  option,  and  nz  x  nz  for  the  Kalman  filter  option.  Prior 
to  the  first  run.  A,  B,  and  C  are  cleared,  and  0  and  R  are  set  to  Identify 
matrices.  If  a  matrix  is  not  input,  its  previous  setting  does  not  change. 
Note  that  a  read/no-read  card  must  be  provided  for  each  matrix. 

Output  Data 

The  print  flag  (IPRTF)  setting  is  used  to  select  the  normal  or  extended 
print-out  options.  A  brief  description  of  the  print  out  data  follows: 

(i)  Normal  output  (IPRTF  »  0) 

1.  Title  field  characters  and  control  flag  settings  (Input  card  No.  1) 

2.  Input  Data  (A,  B,  C,  0,  and  R  matrices) 

3.  Eigenvalues  of  A 

4.  Q'  (-  CT  0  C)  if  IOREGF  «  1  (output  regulator) 

5.  Eigenvalues  of  the  Hamiltonian  (closed  loop  eigenvalues) 

6.  LQR  or  KF  gains 

(ii)  Extended  (Xitput  (IPRTF  ■  1) 

Items  1-6  in  normal  output 

7.  Hamiltonian  matrix 

8.  Packed  eigenvectors  of  H-matrix  from  EISPACK  subroutine  H0R2 

9.  Normalized  eigenvectors 

T  T 

10.  Positive  eigenvector  matrices  j 

11.  Rlccati  equation  solution 


mmm 


•  m  »  «  Va  •  a  a  ^  *  -  %  -  • 


Computational  Details 

The  A-matrix  eigenvalues  are  computed  by  calling  the  EISPACK  subroutine 
BALANC,  ELMHES,  ELTRAX1,  and  HOR2.  The  latter  subroutine  also  computes  the 
eigenvectors,  which  are  not  needed.  HQR,  which  computes  the  eigenvalues  only, 
can  be  used  in  place  of  HQR2  (with  the  call  to  ELTRAN  deleted). 

All  eigenvalues  and  eigenvectors  of  the  Hamiltonian  (HAM)  are  computed  by 
calling  the  EISPACK  subroutines  BALANCE,  ELMHES,  ELTRAN  H0R2  and  BALANC. 
However,  only  the  positive  (or  negative)  eigenvalues  and  eigenvectors  of  HAM 
need  actually  to  be  computed.  An  alternate  strategy  could  be  used,  for 
example,  to  reduce  storage  requirements  or  execution  time  (see  [6]  for 
details).  Note  that  the  subroutine  H0R2  will  fall  for  repeated  eigenvalues. 

The  eigenvectors  of  HAM  are  normalized  for  print-out  only.  This  step  is 
not  actually  required. 

The  FORTRAN  name  KM  is  used  to  denote  the  control  (K)  or  filter  (P) 
Riccati  equation  variable.  The  linear  system  to  be  solved  is 
W11M  x  KM  -  W21M 
where  W11M  -  wj^  ,,  W21M  - 

KM  is  computed  by  using  the  method  and  subroutines  described  in  [11].  The 
subroutines  used  are  DECOMP  and  SOLVE,  which  were  supplied  by  Dr.  L.  R. 
Anderson  of  Virginia  Polytechnic  Institute. 

These  subroutines  are  variable  dimension,  double-precision  versions  of 
the  subroutines  DECOMP  and  SOLVE  given  in  [11,  Chap.  17].  Note  that  the 
subroutine  SING,  which  is  called  by  DECOMP,  is  also  required  (see 
reference).  Note  also  that  one  could  use  the  appropriate  subroutines  from 
LINPACK  [13]  to  replace  DECOMP  and  SOLVE. 


Additional  Subroutines  Called 


The  following  matrix  handling  subroutines  are  used  throughout  the  Drogram 
and  are  included  in  the  listing. 

PRTMAT  -  prints  out  a  matrix  by  rows 

REAEM  -  reads  in  a  matrix  by  rows 

MULMAT  -  multiplies  two  matrices  Ml  x  M2  and  stores  the  result  in  M3 

MATINV  -  inverts  matrix  Ml  and  stores  the  result  in  M2.  MATINV  calls 
DECOMP  and  SOLVE 

Limitations 

If  the  LQR  or  KF  has  repeated  eigenvalues,  HQR2  will  fail.  This 
limitation  can  be  removed  by  using  the  method  described  by  Laub  [14].  This 
method,  which  uses  the  Schur  vector  approach,  is  dependent  on  the  subroutines 
ORTHES  and  ORTRAN,  which  are  in  EISPACK,  and  HQR3,  which  is  not  (see  reference 
for  details). 

In  the  set  up  of  the  H-matrlx,  we  assumed  that  R  was  diagonal.  This 
assumption  is  generally  true,  because  it  is  difficult  to  determine  what  the 
off-diagonal  elements  of  R  should  be  in  the  LOR  option.  In  the  KF  option  the 
output  noise  elements  are  assumed  to  be  uncorrelated.  When  the  Inverse  of  R 
is  called  for,  it  is  computed  by  inverting  the  diagonal  elements  of  R.  This 
limitation  can  be  easily  removed  by  using  the  subroutine  MATINV  to  invert  R. 

There  are  no  checks  for  proper  dimensions,  plant  model  controllability 
and  observability  (required  for  Riccati  equation  solution  to  exist)  or  for 
proper  matrix  characteristics. 

CONCLUDING  REMARKS 

We  have  described  the  methods  and  software  for  solving  the  LQR  and  Kalman 
filter  problems  for  a  stationary  continuous  or  discrete-time  process.  The 
methods  described  have  been  used  for  aircraft  and  submersible  control  system 


design.  We  use  a  separate,  undocumented  FORTRAN  program  to  compute  the  plant 
dynamics  (A)  and  control  input  (B)  matrices  from  the  vehcle  stahility 
derivatives  and,  mass  and  geometric  properties.  The  candidate  control  or 
filter  design  is  tested  with  a  linear  system  simulator,  or  a  6  degree-of- 
freedom  simulator.  The  latter  programs  are  also  undocumented.  However, 
equivalent  programs  are  commonly  used. 

At  present  the  output  from  one  program  is  manually  manipulated  into  the 
input  for  the  next  program  in  the  design  stage.  With  the  recent  acquistion  of 
time-shared  operating  system  based  computers,  we  intend  to  modify  and  combine 
the  separate  FORTRAN  programs  above  in  order  to  provide  an  integrated 
interactive  aircraft  or  submersible  control  system  design  package. 
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APPENDIX  A 


FORTRAN  Program  Listing 


S3 


C  LINEAR  QUADRATIC  GAUSSIAN  CONTROLLER  DESIGN  PROGRAM 

C  VETO  •  18.  9/21/82 

C  CUT  OUT  SOME  PRINT  IN  "A“  VERS 

C  THIS  VERS  IS  A  COMBINATION  OF  SEVERAL  SEPARATE  VERSIONS 

C  AND  1 HER Eh OR  IS  A  BIT  LENOTHY  V  MESSY.  IE.  .COULD  BE  MODULARIZED 

C  WITH  SOME  ADD'L  EFFORTi  NO  EFFORT  WAS  MADE  TO  OPTIMIZE  FORT  CODE 

C  ADDED  FILTER  &  PREDICTOR  CAINS  FOR  DISCRETE  KF  ON  A/30 

C 

C  DISCRETE  AND  CONTINUOUS  LQR  it  KAL  FILT  COMBINED 

C  CHET  OZJMINA.  D98.  820.  767-3171 

C  COMPUTES:  STEADY  STATE  CAIN  FOR  A  LINEAR  QUADRATIC 

C  REGULATOR  OR  KALMAN  FILTER 

C 

c ******  I  TYPE  »  O  (  LINEAR  QUADRATIC  REGULATOR  >  ****** 

C 

c  ********  CONTINUOUS  SYSTEM  ********* 

C  PLANT  DYNAMICS  XD-A*X*B*U 

C  OUTPUT  PROCESS  Z  -  C  •  X 

C  COST  FUNCTION  J  •  .9  *  INTEGRAL  OF  (  XCT)  *  Q  *  X  ♦ 

C  U<T>  *  R  *  U  >  DT  > 

C  Q  MUST  BE  POS  SEMI-DEF,  SYMMETRIC 

C  R  MUST  BE  POS  DEF.  DIACONALI IN  THIS  PROG) 

C  STATE  OR  OUTPUT  REGULATOR  OPTIONS  AVAILABLE 

C  I ORE OF  -  0/1  (  STATE/OUTPUT  REGULATOR  > 

C  SEE  ATMANS  h  FALB.  CHAP  9  FOR  DETAILS 

C  NOTATION  GENERALLY  FOLLOWS  THAT  OF  ATMANS 

C  MATRICE3  IN  FORT  CODE  GENERALLY  END  IN  M. IE.  A  »  AM.  ETC. 

C  VECTORS  GEN  END  IN  V 

C  88  SOt  TO  THE  RICCATI  EON  IS  OBTAINED  VIA  MACFARLANE— 

C  POTTER  METHOD 

C  EXSPACK  IS  USED  TO  COMPUTE  THE  REQ'D  E-VECTORS 

C  THIS  VBR8  USES  DP  EISPACK 

C  fiECOMP  %  880LVE  ARE  USED  TO  SOLVE  LINEAR  8Y8T  IN  PLACE 

C  OF  MATRIX  INVERSION 

C  NOTE:  BISPACK  SR  MQR2  FAILS  FOR  REPEATED  E-VALUES 

C  **********  DISCRETE  VERSION  <  IDISF  -  1  >  ******* 

C  PLANT  DYNAMICS  X(K*1)  -  A  •  XCK)  ♦  B  *  U(K> 

C  OUTPUT  PROCESS  Z(K)  •  C  *  X(K> 

C  COST  FUNCT  J  -  .9  *  8UM<  X(K) (TRANS)  *  Q  *  X(K>  * 

C  U(K> (TRANS)  *  R  *  U(K)  >.  K  ■  0  TO  N 

C  STATE  OR  OUTPUT  REG  OPTION  (  IOREOF  -  0/1  ) 

C  NOTE:  A.S.Q.R  ARE  DISCRETE  EQUIVALENTS  OF  CONT  PLANT 

C  REF:  PAPER  BY  DORATO  it  LEVIS.  IEEE  TRANS  ON  AUTO  CNTRL. 

C  PP  613-630.  DEC.  1971 

C *******  I TYPE  *  1  <  STEADY  STATE  KALMAN  FILTER  )  ******* 

C 

C  PLANT  it  0B8ERV  MODELS: 

C  XD>A*X*W«Z»C*X  +  V,  WHERE  W.  V  ARE 

C  INDEPENDENT  0AU8IAN  WHITE  N0IPR0CES8ES 

C  WITH  INTENSITY  MATRICES  OF  Q  »>  O. R  >  0  . 

C  88  KA1  SAIN  0  -  K  •  C(T>  •  R(INV>.  WHERE 

C  K  •  SS  SOL  TO  THE  FILTER  RICCATI  EON 

C  DISCRETE  FILTER  CASE: 

C  X(K>1)  >  A  •  X(K>  *  W(K) «  Z(K>  -  C  *  X(K>  *  V(K) 

C  WHERE  A.B.C.Q.R  ARE  DISCR  EQUIVALENTS  OF  CONT  PROCESS 

C  SS  PZITCR  GAIN  18:  0F(SS)«  K(8S>*C(T)*CC*K(88)*CtT>*RltINV) 

C  SS  KAl  PREDICTOR  GAIN  IS  0P(S8)  •  A  *  OF(BS) 

C  WHERE  KISS)  IS  THE  SS  SOL  TO  THE  DISCR  RICCATI  EON 

C  NOTE:  THE  DUALITY  THEOREM  IS  USED  TO  SOLVE  THE  FILTER 

C  PROM.  EM  VIA  THE  SAME  METHOD  AS  THE  LOR  PROBLEM 

C  REFS:  ASTNOM.  K.  J.  "INTRODUCTION  TO  STOCHASTIC  CONTROL 


C  THEORY".  CR  KALMAN'S  OR I®  PAPERS.  OR  THE  EXCELLENT 

C  TUTORIAL  BY  I.  B.  RHODES  IN  IEEE  TRANS  ON  AUTO  CCNTR. 

C  DEC  1971, PP.  688-706. 

C 

C *********  IPRTF  •  1,  ADD 'L  PRINT  OUT  TOR  DEBUQGINO  ***** 

C 

IMPLICIT  REAL*8  <A-H,  K-Z> 

DIMENSION  AM(  19,  19),  BMUS,  19),  CM(15.  19),  0M(  19.  19).  RHUS.  19) 
DIMENSION  HAM<30, 30),  PM< 19,  lSI.OPMClS, 1S),SM<19,  IS), TMI19, IS) 
DIMENSION  AIH<  19,  19).  HUMUS,  19).  W21MUS.  19), WM<30.  30),  ZN<30.  30) 
DIMENSION  WRV(30).  WIV(30>,  TV<30).  TV1 ( IS). TV2(1S> 

INTEGER  IV1(30>.  IVSOO) 

INTEOER  1NX,  INU,  INZ.  INH,  INXMX,  INUMX,  INZMX. INHMX 
REAL**  1 I1LE( 19) 

C 

C 

C  CLEAR  SOME  VARIABLES 

C 

ICASE  *  0 
IDATAF  »  O 
IRAF  -  0 
IRBF  «  0 
IRCF  »  O 
IROF  -  0 
IRKF  -  0 
I OR EOF  *  O 
IPRTF  *  0 
IDISF  >  O 
I TYPE  *  O 
C 

INX  -  0 
INU  -  O 
INZ  *  O 
INH  -  O 
I  NR  *  O 
C 

C  SET  MAX  DIMENSIONS 

C 

INXMX  -  15 
INUMX  *  INXMX 
INZMX  -  INXMX 
INHMX  »  INXMX  *  2 
C 

C  CLC-AR  SOME  ARRAYS 

C  SET  OH.  RM  TO  UNITY  MATRICES 

C 

DO  10  JO,  INXMX 
DO  9  I* 1  •  INXMX 
AM(I.J)  *  0.  ODO 
BM(I,J)  -  0.  ODO 
CH(I,  J)  ■  0.  ODO 
QM( I.  J>  »  O.  ODO 
QPM( I,  J)  •  0.  ODO 
RM<  X#  J)  >  0.  ODO 
9  CONTINUE 

QM(J,  J>  »  1.  ODO 
RHIJ, J)  «  1.  ODO 
10  CONTINUE 
C 

C  READ  ALL  INPUT  DATA  HERE 

C 

C  IDATAF  *  READ  DATA  FLAOCl/O) 

C  X  ORE  OP  •  OUTPUT  REGULATOR  FLAG,  NE*-D  C-MTRX  IF  0NC1) 

C  IDISF  •  DISCRETE  REGULATOR  FLAG 

C  IPRTF  •  OPTIONAL  PRINT  FLAO,  PROVIDES  MORE  DATA  FOR 

C  DEBUGGING 

C  I TYPE  ■  KALMAN  FILTER  FLO,  COMPUTE  S8  KALMAN  OAIN  IF 

C  0N<1> 

C  IRAF  *  READ  A-MTRX,  IRBF  -  READ  B-MTRX,  ETC. 

C  NOTE:  1 'ST  2  INPUT  CARDS  ARE  MANDATORY  FOR  EVERY  CASE 

C  COMMENTS  ON  CARD  «1  BEGINNING  AT  COL  *  ON  ARE  PRINTED 

C  INPUT  «  OF  ELEMENTS  IN  X, U. Z  ON  CARO  G2 

C  CARD  CONTAINING  INPUT/NO  INPUT  PLAC  MUST  PRECEDE  EACH  MATRIX. 
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C  WHETHER  OR  NOT  THE  MATRIX  IS  INPUT,  ORDER  OF  MATRIX  INPUTS  IS 

C  A.  D.C.O, Ri  ANY  COMMENTS  ON  MATRIX  INPUT/NO  INPUT  FLAG  CARD 

C  FROM  COL  6  ON  ARE  NOT  READ  IN 

CC  MATRIX  READ  FORMAT  IS  *E!D *  12.  8. DEP  ON  VERS.  IE.  . SINGLE  OR  DP 

C  NEW  CARD  FOR  EACH  ROW 

C 

1  READ (9<  901 )  IDATAF, IOREGF.  IDISF,  IPRTF.  ITYPE.  (TITLE(I).  1*1.  15> 

901  FORMAT! 5Ili  19A4) 

C  TEST  FOR  NEW  CASE 

IF( IDATAF  .EG.  0  )  GO  TO  9999 
C  READ  DIMENSIONS  OF  AM. BM. CM 

READ! 9. 70?)  INX,  INU,  INZ 

902  FORMAT (SI?) 

C  TEST  FOR  NEW  A-MTRX 

READ <5.  901 >  IRAF 
IF < IRAF  EO.  0  ) GO  TO  29 
CALL  RE ADM ( INXMX.  INX.  INX,  AM) 

C  TEST  FOR  NEW  B-MTRX 

29  READ! 9.  901)  IR8F 

IF! 1RBF  EO.  0  )  00  TO  39 
CALL* READM! INXMX,  INX.  INU,  BM) 

C  TEST  FOR  C-MTRX 

39  READ! 9,  901)  IRCF 

IF! IRCF  .EO.  0  )  GO  TO  49 
CALL  READM! INZMX,  INZ.  INX,  CM) 

C  TEST  FOR  FEW  Q-MTRX 

49  READ!9<  901 )  IRGF 

IF! IRGF  EO.  O  )  GO  TO  99 
CALL  READM! INXMX.  INX.  INX.  0M> 

C  TEST  FOR  NEW  R-MTRX 

C  INR  IS  DIMENSION  OF  R-MTRX  TO  BE  READ  IN 

C  SET  INR  -  INU  FOR  LOR  CASE,  THEN  TEST  FOR  KAL  FILT 

99  INR  -  INU 

IF!  ITYPE  .  NE.  0  )  INR  -  INZ 
READ! 9.  901)  IRRF 
IF! IRRP  EO.  0  >  GO  TO  49 
CALL  READM! INUMX.  INR.  INR.  RM) 

C  DATA  IS  READ  IN 

69  I CASE  >  ICA3E  *  1 

C 

C  NOW  PRINT  INPUT  DATA! OR  DEFAULT  DATA) 

C 

WRITE!*.  915)  ICASE 

919  FORMAT! 'I',  10X.  'INPUT  DATA  FOR  CASE  NO.  14) 

WRITE !6.  717 >  ! TITLE! I >.  1*1,  19) 

917  FORMAT! 'O'.  5X.19A4) 

WRITE!*.  71*0) 

920  FORMAT! 'O'.  SX.  'FLAGS! I -YES, 0-N0> ') 

WRITE!*.  925)  IRAF. IRBF,  IRCF. IRGF.  IRRF 

929  FORMAT!  '  NEW  A*'.  12.  9X.  'NEW  8-'.  12.  9X.  'NEW  C-'.  12.  9X.  'NEW  G-'.  12. 

1  9X. 'NEW  R» ', 12) 

WRITE!*.  930)  IOREGF.  IDISF.  IPRTF,  ITYPE 

930  FORMAT!'  OUTPUT  REG  FLO*'.  12. 9X.  'DISCRETE  REG  FLO-'.  12. 

X  /.  '  OPTIONAL  PRINT  FLO  *'.  12.  9X,  'KALMAN  FILTER  FLO  12) 

IF!  IRAF.  EO.  0  )  00  TO  67 
WRITE!*.  933)  INX.  INX 

933  FORMAT!  'O'.  5X.  'A-MTRX  12.  '  BY  12) 

CALL  PRTMAT ! INXMX.  INX,  INX.  AM) 

67  IF!  IRBF  .  EO.  0  )  00  TO  *9 
WRITE!*.  935)  INX.  INU 

939  FORMAT! 'O'.  5X.  'B-MTRX  '.12,  '  BY  Mil 
CALL  PRTMAT ! INXMX,  INX.  INU.  BM) 

69  IF!  IRCF  .EO.  0  )  00  TO  71 
WRITE!*.  730)  INZ.  INX 

930  FORMAT!  'O',  5X,  'C-MTRX  ',  12.  '  BY  ',  12) 

CALL  PRTMAT  !  INZMX,  INZ,  INX,  CM) 

71  IF!  IROF  .EO.  0  )  00  TO  73 
WRITE!*.  740)  INX,  INX 

940  FORMAT!  'O',  OX,  'O-MTRX  ',12,'  BY  Mil 
CALL  PRTMAT (INXMX.  INX.  INX.  OH) 

73  IF!  IRRF  .  EO.  0  )  00  TO  79 
WRITE!*,  942)  INU.  INU 


ouu  uuuoouuu  o  u  o  o  ouo  o  u  u u 


942  FORMAT (  'O'.  5X,  'R-MTRX  ',12.'  BY  ',12) 

CALL  PRTMAl ( INUMX,  INR,  INR, RM> 

WRITE!*.  945) 

945  FORMAT ('  CAUTION!!!  R-MTRX  MUST  BE  DIACONAL  DUE  TO  METHOD  OF' 
1  '  INVERT ING! ! ! ' ) 

C 

C *******  COMPUTE  S-VAL'S  OF  A-MTRX  *********** 

SET  TM  *  AM  TO  PRESERVE  A-MTRX 
USE  EISPACK  TO  COMPUTE  E-VAL'S  OF  TM 

fcOTE:  WE  ARE  USING  HGR2  INSTEAD  OF  HQR  FOR  CONVENIENCE 
75  DO  85  1*1, INX 
DO  85  J* J , INX 
85  TM(I.J)  *  AM <  I ,  J > 

CALL  HAL ANC( INXMX,  INX,  TM,  ILOU,  IHI,  TV) 

CALL  ELMIIES!  INXMX,  INX,  ILOU,  IHI,  TM,  IV1  > 

CALL  ELTRAN! INXMX,  INX,  ILOU.  IHI.  TM,  IVt.SM) 

CALL  MGR**!  INXMX,  INX,  ILOU,  IHI,  TM,  URV,  UIV.  SM.  I  ERR  > 

IF <  II- RR  .EG.  0  )  GO  TO  90 
WRITE  (A.  748)  I  ERR 

948  FORMAT! 'O', 5X, 'EIGENVALUE  COMPUTATION  FAILURE.  I ERR  -'.12) 

GO  TO  100 

90  WRITE!*.  749) 

949  FORMAT! 'O'.  SX.  'THE  EIGENVALUES  OF  THE  A-MTRX  ARE:  ') 

DO  100  1*1. INX 

WRITE !*. 960)  URV! I > .  WIVI I ) 

100  CONTINUE 

*******  BEGIN  COMPUTATIONS  TO  SET  UP  HAMILTONIAN  ********** 

COMPUTE  GPMi  STATE  REG:  GPM  -  G 

OUTPUT  REG:  GPM  -  C!T>  *  0  *  C 
KAL  FILTER:  GPM  -  Q 

TEST  FOR  KAL  FILT 
IF!  ITYPE  .NE.  0  >  GO  TO  130 
TEST  I  ORE  OF 

IF! IORECF  .NE.  0  )  GO  TO  150 

SET  GPM  «  ON  FOR  KAL  FILT  OR  STATE  REGULATOR 
130  DO  140  U> 1.  INX 
DO  140  1*1.  INX 

GPM! I.  J)  *  QM! I.  J) 

140  CONTINUE 
GO  TO  130 
150  CONTINUE 

COMPUTE  GPM  -  CHIT)  *  OH  *  CM 
DO  160  I'  1, INX 
DO  160  U> 1, INX 
WM!I.J>  *  O.  ODO 
DO  160  IK  •  1,  INX 

UM(I.J)  «  WM< I,  J)  *  QMII, IK)  •  CM! IK,  J) 

160  CONTINUE 

NOW  COMPUTE  GPM  -  CHIT)  •  WM 
DO  170  1*1.  INX 
DO  170  J* 1.  INX 
GPM! I.  J)  *  O.  ODO 
DO  170  IK* 1,  INX 

GPM(I.U)  •  GPM! I.  J)  *  CM! IK. I)  •  WM! IK,  U> 

170  CONTINUE 

SET  DIMENSION  OF  HAMILTONIAN 

180  INH  -  INX  *  2 

TEST  FOR  KALMAN  FILTER 
IF!  ITYPE  .NE.  0  )  00  TO  110 

COMPUTE  SM  -  BH  *  RM(INV)  •  BM!T> 

FIRST.  COMPUTE  TM  -  RM(INV)  *  BM!T>,  SAVE  FOR  USE  LATER 
DO  175  1*1.  INU 
DO  175  U* 1.  INX 

TMU.J)  •  BM!J.  I)  /  RHII,  I) 

175  CONTINUE 
C  NEXT  COMPUTE  SM  -  BM  *  TM 

CALL  MUl MAT! INXMX,  INX.  INU.  BM,  INXMX.  INX,  TM.  INXMX.  SM) 


SM  *  CH<T )  *  RM  *  CM  FOR  KALMAN  FILTER 

ItO  DO  120  I*  1,  INZ 
DO  11*0  J*  1 ,  INX 

TM(  J.  I)  -  CMC,  J)  /  RM<  I.  I> 

120  CONTINUT- 

COMPUTE  SM  -  TM  *  CM 

CALL  MULMATI INXMX.  INX.  INZ.  TM.  INXMX.  INX.  CM,  INXMX, SM) 

TEST  FOR  KALMAN  FILTER 
133  IF <  I TYPE  .NE.  0  )  GO  10  460 
>•••••«**  RL- GUI  AT  OR  PROBLEM  ********* 

TEST  POR  CONTINUOUS/DISCRETE  LQR 


IF <  IDISF  .  NE.  0  )  GO  TO  400 

•»**•»»*  CONTINUOUS  PLANT  LQR  ******* 

FORM  THE  CONTINUOUS  PLANT  HAMILTONIAN 


C  HAM  *  -AM  .  SM 

C  OPM  .  AMC  T ) 

C  WHERE  SM  -  BM  *  RMIINV)  *  BM<T> 

C  OPM  »  QM  OR  CM(T!  *  QM  *  CM.  DEPENDING  ON  IOREGF 

C 

C  MOVE  -AM  INTO  UPPER  LFFT  BLOCK  OF  HAM, 

C  OPM  INTO  LOWER  LEFT  BLOCK. 

C  SM  INTO  UPPER  RT  BLOCK 

C  AM(T)  INTO  LOWER  RIGHT  BLOCK 

C 

DO  190  l» 1,  INX 
11-1  ►  INX 
DO  190  J* 1.  INX 
Jl  -  J  ►  INX 
HAH< 1. J)  «  —AMI I.  J) 

HAM< 1.  J1 >  -  SMI  I.  J) 

HAMtll.  J)  -  OPM 1 1,  J) 

HAM!  II.  «ii>  -  AMIJ,  I) 

190  CONTINUE 

00  TO  450 
C 

C *******  FORM  DISC RE IE  PLANT  HAMILTONIAN  ********* 


AMIINV) 

OPM  *  AIINV) 


AMIINV)  •  SM 

AIT)  *  OPM  •  AMIINV)  •  SM 


WIERE  SM  -  BM  *  RMIINV)  •  BMIT) 

400  CONTINUE 

COMPUTE  AMIINV)  BY  LU  DECOMPOSITION 

SET  FURTHER  DOWN  IN  LISTINO  FOR  REF  «•  DETAILS 

CALL  MAT INVI INXMX.  INXMX.  INXMX.  INXMX,  INX.  AM.  KM.  W1 1M. 

X  W21M,  1VI.  1V1) 

AMIINV)  -  KM 

COMPUTE  AMIINV)  *  SM  -  W11M 

CALL  MULMATI INXMX,  INX. INX,  KM.  INXMX.  INX.  SM.  INXMX.  W11M) 
COMPUTE  OPM  •  AMIINV)  •  SM  -  W21M 
CALL  MULMATI INXMX,  INX.  INX.  OPM.  INXMX,  INX.  WilM.  INXMX.  W21M) 
COMPUTE  AMIT)  ♦  OPM  *  AMIINV)  *  SM. 

STORE  IT  IN  LOWER  RT  BLOCK  OF  HAM 
DO  420  M.  INX 
II  *  INX  *  I 
DO  420  J* 1. INX 
J1  •  INX  *  J 
HAM 1 1,  J)  -  KM 1 1,  J) 

HAMIT,  Jl)  *  WUMII.J) 

HAMtll. Jl)  -  AMIJ.  I)  ♦  W21MII.J) 

420  CONTINUE 

COMPUTE  OPM  •  AMIINV) 

CALL  MULMATI INXMX.  INX.  INX,  OPM,  INXMX,  INX.  KM.  INXMX.  W21M) 
STORE  IN  LOWER  LEFT  BLK  OF  HAM 
DO  430  1*1.  INX 
II  *  INX  *  1 


non  non  no  nnnnnono  nnnnnnnnnnnn  non 


DO  400  J* 1 . INX 

HANOI.  J)  «•  W2 1 M  (  I ,  J) 

430  CONTINUE 

C  PRINT  OUT  QPM  IF  I0REGF  IS  SET 

430  IF<  I0RI-GF  .  EQ.  0  )  GO  TO  500 

IFCIRGF.  AND.  IRRF.  EQ.  0)  GO  TO  500 
WRITEC6.  746)  INX.  INX 

946  FORMATC  'O'.  5X,  'CCT)  *  O  *  C  MATRIX  12.  '  BY  '.  12) 
CALL  PRT MAT  < INXMX.  INX,  INX. QPM) 

GO  TO  000 

KALMAN  FILTER  HAMILTONIAN  ******* 


TEST  FOR  DISCRETE  KAL  FILT 
460  IF (  IDISF  .  NE.  0  )  GO  TO  480 
********  CONTINUOUS  PLANT  KAL  FILT  ******* 
FORM  THE  CONTINUOUS  PLANT  HAMILTONIAN 


HAM  *  -AM  <  T ) ,  SM 
OPM  .  AM 

WHERE  SM  -  CM(T)  *  RHCINV)  *  CM 


MOVE  -AM<T>  INTO  UPPER  LEFT  BLOCK  OF  HAM. 
OPM  INTO  LOWER  LEFT  BLOCK, 

SM  INTO  UPPER  RT  BLOCK 
AM  INTO  LOWER  RIGHT  BLOCK 

DO  470  I‘  1.  INX 
Il-l  ►  INX 
DO  470  J- 1 .  INX 
Jl  *  J  *•  INX 
HAM( I, J)  •  — AH< J,  I) 

HAM(I.Jl)  »  SMC  I,  J) 

HAMCIl.  J)  -  QPMCI,  J) 

HAMCU.  Jl)  m  AMCI.  J) 

470  CONTINUE 

GO  TO  OOO 

•••**•  FORM  DISCRETE  PLANT  HAMILTONIAN  ********* 


HAM  *  AM(tNV-T) 

OPM  *  ACINV-T) 


AMCINV-T)  •  SM 
A  *  QPM  *  AMCINV-T)  *  SM 


WICERE  SM  -  BH  *  RMCXNV)  •  3MCT) 

INVERT  AM  BY  LU  DECOMPOSITION 
480  CALL  MAT 1NV( INXMX.  INXMX,  INXMX. INXMX.  INX. AM. AIM.  WI1M. 

Y  W21M.  TVI.  IVl) 

AMC INV)  ■  AIM 

FIRST.  TRANSPOSE  AIM 
DO  403  l*  St,  INX 
xi-  r  l 

DO  405  X.  XI 
DUM  •  AIMCI.J) 

AIMCI.J)  >  AIMCJ.  I) 

489  AIMC  J.  I >  *  DUM 
COMPUTE  OPM  *  AMCINV-T) 

CALL  MULMATC INXMX.  INX.  INX.  OPM.  INXMX.  INX.  AIM.  INXMX. KM) 
COMPUTE  AMCINV-T)  •  SM 

CAll  MULMATC INXMX.  INX. INX.  AIM.  INXMX.  INX.  SM.  INXMX.  W11M) 
COMPUTE  OPM  *  AMCINV-T)  *  SM  -  W21M 
CAL'.  MULMATC  INXMX.  INX.  INX.  QPM.  INXMX.  INX.W11M.  INXMX.  W21M) 
COMPUTE  AM  *  QPM  •  AMCINV-T)  *  SM. 

ST(*E  IT  IN  LOWER  RT  BLOCK  OF  HAM 
STORE  REMAINING  ELEMENTS  IN  HAM  IN  SAME  LOOP 
DO  490  I- 1. INX 
II  •  INX  *  I 
DO  470  J-  1 ,  INX 
J!  >  INX  *  J 
HAHCl.J)  •  AIMCI.J) 

HAMCI.J1)  *  WUMCI.J) 

HAMCIl. J)  •  KMC  I.  J) 

HAMCIl.  Jl)  -  AMC I.  J)  *  waiMCI.J) 

490  CONTINUE 

C  OPTIONAL  PRINT  OUT  OF  HAMILTONIAN  MATRIX 

C 
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900  IFdPRTF  .EG.  O  )  GO  TO  195 
WRITEI6, 74?)  INH,  INH 

947  FORMAT! 'O'. SX.  'HAMILTONIAN  MATRIX  ',12,  '  BY  '.  I2> 

CALL  PRTMAT < INHMX,  INH,  INH,  HAM) 

199  CONTINUf 

C  NOW  COMPUTE  THE  E- VALUES  AND  E- VECTORS  OF  HAM 

C  VIA  E ISPACK  SUBROUTINES 

C 

CALL  BAIANC! INHMX,  INH,  HAM.  ILOW,  IHI,  TV) 

CALL  EL MHES! INHMX,  INH,  ILOU,  IHI.  HAM,  IV1) 

CALL  EL  TRANdNHMX.  INH,  ILOW.  IHI,  HAM,  I VI,  ZM> 

CALL  HOf«*{ INHMX,  INH,  ILOW, IHI.  HAM.  WRV.  WIV,  ZM.  I ERR > 

IF < IERR  EG.  0  )  GO  TO  200 
WRITEI6,  950)  IERR 

990  FORMAT< 'O'. 5X,  'EIGENVALUE  COMPUTATION  FAILURE.  IERR  -  '.  12) 
GO  TO  1 

200  CALL  BAL  BAM  INHMX,  INH.  ILOW.  IHI.  TV.  INH.  ZM) 

C 

C  PR INI  E -VALUES 

WRITE!  6.  755) 

999  FORMAT ( 'O'. 5X,  'THE  EIGENVALUES  OF  THE  HAMILTONIAN  ARE:  ') 

DO  210  1*1.  INH 

C  PRINT  NEC  E-VAL'S  ONLY 

IF<  WRVd)  .  GT.  0.  0  )  GO  TO  210 
WRITE !6, 960)WRV! I ).  WIV( I > 

960  FORMAT ( '  '. 1PD19.  7, 10X. IPD15.  7) 

210  CONTINUE 

C  OPTIONAL  PRINT  OF  PACKED  E-VECTORS  FROM  EISPACK 

IFdPRTF  .EG.  0  )  GO  TO  21 2 
C  PRINT  THE  PACKED  E-VECTORS  OF  HAM 

WRITEI6.  962) 

962  FORMAT < 'O',  SX.  'THE  E-VECTORS  OF  HAM  ARE:  '» 

CALL  -PRTMAT  < INHMX.  INH.  INH. ZM) 

212  CONTINUE 
C 

C  NORMAL  1 7E  THE  E-VECTQRS 

C 

IK  -  O 

214  IK  •  IK  U 

IF (  WIVdK)  .  NE.  0.  ODO  >  00  TO  220 
SUM  -  O.  ODO 
DO  219  1*1. INH 

SUM  -  SUM  ►  ZM(I.IK)  *  ZM(I.  IK) 

219  CONTINUI- 

SUM  •  SORT  <  SUM  ) 

DO  218  1*1. INH 
ZM<I. IK)  •  ZM(I. IK)  /  SUM 
218  CONTINUE 
GO  TO  200 
C 

220  SUM  *  0.  ODO 

DO  229  1* 1, INH 

BUM  -  SUM  *•  ZM(I.IK)  *  ZM<I.  IK)  ♦  ZMd.IK+l)  *  ZMd.  IK+l) 

229  CONTINUE 

SUM  -  SORT!  SUM  > 

DO  228  Id,  INH 
ZM(I.IK)  *  7M(I.  IK)  /  SUM 
ZM(MKH)  •  ZM(  I.  IK+1 )  /  SUM 
228  CONTINUE 

IK  -  IK  »  1 

230  IF<  IK  .11.  INH  )  GO  TO  214 

C  OPTIONAL  PRINT  OF  NORMALIZED  E- VECTORS  FROM  EISPACK 

IFdPRTF  .EG.  0  )  GO  TO  239 
WRITEC6,  761  > 

961  FORMAT! 'O'. SX.  'THE  NORMALIZED  E-VECTORS  OF  HAM  ARE:  ') 

CALL  PRTMAT! INHMX,  INH,  INH.  ZM) 

239  CONTINUt- 

C  RE-ARRANOE  THE  E-VECTORS 

C  TEST  FOR  CONTINUOUS/DISCRETE  CASE 

IF(  ID ISP  .  NE.  0  >  GO  TO  260 
C 

C  SELECT  THE  E-VECTORS  ASSOCIATED  WITH  POS  E-VALUES 

C  AND  PUT  THEM  INTO  THE  PARTITIONED  MATRICES  W11M.U21M 

C  TRANSPOSE  W11M.W12M 


on  nnnnnnnonn  o  no  non  o  o  non  nnnnn 


DO  290  IK* 1. INH 

IF(WRV( IK)  .  LT.  0.  ODO  >  00  TO  290 
NO  POS  £ -VALUE 

IF  E  -VALUE  IS  COMPLEX.  THEN  TWO  E-VECTOR 
2  COl  'S  ARE  REQ ' D.  H0R2  COMPUTES  POS  PART  ONLY 
SINCE  NEQ  PART  IS  CONJUGATE. WE  WIU  PICK  UP  2ND 
COl  ON  NEXT  PASS 

DO  240  I>1. INX 
II  -  I  ►  INX 
wumj.  I)  •  ZM(  I.  IK) 

W21M( J,  I)  -  ZM( 1 1.  IK) 

240  CONTINUE 
290  CONTINUE 

60  TO  poo 

RE -ARRANGE  E-VECTORS  DIFFERENTLY  FOR  DISCRETE  CASE 
REF:  VAUGHN.  IEEE  TRANS  ON  AUTO  CONT.  .  OCT  1970 

260  J  •  O 

DO  270  IK  »  1,  INH 
C0MPU1E  MAO  OF  E-VAL 

SUM  *  WRV(IK)  •  WRV(IK)  +  WIV(IK)  •  WIV(IK) 

SUM  *  DSQST (  SUM  ) 

TES1  FOR  E-VAL  OUTSIDE  OF  UNIT  CIRCLE 
IF(  SUM  .LT.  1.  ODO  >  GO  TO  270 

E-VAl  IS  OUTSIDE  OF  UNIT  CIRCLE.  GET  6  STORE  E-VECT 
IF  £  -VAL  IS  COMPLEX.  WE  WILL  PICK  UP  OTHER  PART  OF 
E-VEC1  AUTOMATICALLY  ON  NEXT  PASS 
J  -  J  >  1 
DO  270  I  »  1,  INX 
It  ■  I  ►  INX 
WllMtJ, l)  -  ZM( I.  IK) 

W21MIJ. 1)  -  ZM (It. IK) 

270  CONTINUE 

OPT  ZONAL  PRINT  OF  POS  E-VAL  EIO  VECTORS 
280  IF(IPR1F  .  EG.  0  )  GO  TO  290 
PR  IN')  U11M.W21M 
WRITE(6.  970) 

970  FORMAT!  'O'.SX,  'Wll(T)  IS') 

CALL  PR1MA1 (INXMX. INX.  INX.W11M) 

WRITE (6.  77?) 

972  FORMAT! 'O'.SX. 'W21<T>  IS') 

CALL  PR1MA1! INXMX. INX.  INX.W21M) 

290  CONTINUE 

LOR  SS  GAIN  G  -  -RUNV)  *  BIT)  *  K 
CONI  KAl  FILT  SS  GAIN  *  K  *  C!T>  *  R!INV) 

WHERE  K  *  SS  SOL  TO  ALO  RICCATI  EQN 
VIA  POUEK'S  METHOD 
Wild  )  *  K  -  W21  !T> 

SOLVE  FOR  COLS  OF  K  BY  LU  DECOMP  METHOD 
REF:  FORSYTHE  fc  HOLER  "COMPUTER  SOLUTION  OF  LINEAR 
ALGEBRAIC  SYSTEMS" 

FI RSI  DO  LU  DECOMPOSITION 
CALL  DECOMP ! INX.  WtlM.  INXMX.  ZM.  INHMX.  IV1.TV1) 

NOW  COMPUTE  COL'S  OF  KM 

DO  300  J'  1,  INX 

CALL  8S0L VE ( INX.  ZM.  INHMX,  W21M! 1.  J),  KM! t.  J) .  IV1 ) 

300  CONTINUE 
C  TES1  FOR  KAL  FILT 

IF!  HYPE  .  NS.  0  >  GO  TO  310 
C  TEST  FOR  ADD'L  PRINT  OUT 

IF!  IPR1F  .EG.  0  )  GO  TO  318 
C  PR INI  OUT  R! INV)  •  B!T> 

WRITE!6. 975)  INU,  INX 

979  FORMAT! 'O',  SX.  'R!  INV  )  *  B!  TRANS  )  MATRIX'. 12,  '  BY  M2) 
CALL  PR1MA1! INXMX.  INU,  INX,  1M) 

GO  TO  310 

C  TEST  FOR  ADD'L  PRINT  OUT 

310  IF!  IPR1F  .  EG.  0  )  GO  TO  318 


non  non  n  n  n  on 


C 


PRINT  OUT  C <T>  #  R <  I NV >  MATRX  IN  Ttl 
WRITE(6.  976)  INX, INZ 

976  FORMAT! 'O'.  5X.  'C(T)  *  R(INV>  MATRIX',  12.  '  BY  ',12) 

CALL  PRTMAT <  INXMX,  INX,  INZ.  TM) 

C  NOVI  PRINT  OUT  SS  K-MTRX 

319  WRITE(6.  980) 

980  FORMAT < 'O'.  SX.  'SS  K-MTRX<RICCATI  EON  SOLUTION  IS') 

CALL  "PRTMAl ( INXMX,  INX,  INX, KM) 

318  MR I TE  <  6,  985 ) 

989  FORMAT! 'O'. SX. 'THE  SS  GAIN  MTRX  C  IS  : ') 

C  TEST  FOR  KAL  FILT 

IF<  IT YPE  .  N£.  0  >  GO  TO  320 
C  COMPUTE  SS  LOR  GAIN:  R(INV>  •  B ( T )  IS  IN  TM 

CALL  MUi MAT (INXMX.  INU,  INX. TM.  INXMX.  INX. KM.  INHMX,  WM> 

CALL  PRTMAT  < INHMX.  INU.  INX.WM) 

GO  TO  1 

320  IF<  IDISF  ME.  0  )  GO  TO  329 

C  COMPUTE  SS  KAL  FILT  GAIN:  C(T>  *  R(INV)  IN  TM 

CALL  MULMAT (INXMX,  INX.  INX. KM.  INXMX.  INZ. TM.  INHMX, UM> 

C  PRINT  SS  GAIN  MTRX 

WRITE(6.  980) 

988  FORMAT ( '  '.SX. 'O(SS)  »  K(SS>  *  C(T)  *  R(INV)  FOR  CONT  KF'I 
CALL  PRTMAT (INHMX.  INX,  INZ. MM) 

GO  TO  1 

COMPUTE  DISCR  KF  SS  GAIN  0-K*C ( T)*CC*K*C (T)+R3 ( INV) 
FIRST  COMPUTE  «i  SAVE  K*C(T> 

329  DO  330  Z> 1 . INX 
DO  330  J* 1 . INZ 

Ml 1M( I. J)  •  O.  ODO 
DO  330  IK* 1.  INX 

330  HUM(I.J)  -  WUM(t.J)  ♦  KM(l.IK)  *  CMC  J,  IK) 

CALL  MU.  MAT(  INXMX.  INZ,  INX.  CM.  INXMX.  INZ,  HUM.  INXMX.  W21M) 
DO  335  1*1. INZ 

339  W21M( I.  I)  -  W21M(I,  I)  ♦RM(I.I) 

INVERT  M21M 

CALL  MAT INV< INXMX.  INXMX.  INHMX.  INHMX.  INX. W21M.  AIM. MM.  ZM. 

X  TV1. IV1  ) 

COMPUTE  OF (88)  -  Willi  *  AIM 

CALL  MU  MAT  (INXMX.  INX.  INZ.  HUM.  INXMX.  INZ.  AIM.  INXMX.  U21M) 
WRITE (6, 9V0) 

990  FORMAT! 'O'.  SX.  '88  FILTER  Op (88)  IS:  ') 

CALL  PRT MAT ( INXMX .  INX.  INZ.  W21M) 

COMPUTE  KF  PREDICTOR  SS  OAIN  GP(SS>  -  A  *  OF(SS) 

CALL  MU MAT( INXMX.  INX.  INX.  AM. INXMX. INZ.  W21M.  INXMX.  W11M) 
WRITE (6.  992) 

992  FORMAT! 'O'. SX.  'SS  PREDICTOR  GAIN  IS:  ') 

CALL  PRT  MAT ( INXMX.  INX.  INZ,  HUM) 

00  TO  1 
999  CONTINUE 
END 

SUBROUTINE  PRTMAT! IRMAX.  IR, IC.  MAT) 

PRINTS  OUT  A  MATRIX  BY  ROWS 

IMPLICIT  REAL *8  (A-H.K-Z) 

DIMENSION  MAT! IRMAX,  1) 

DO  10  I*  ».  IR 

WRITE (6,  90)  (MAT(I.  J),  J»l.  IC) 

10  CONTINUE 

90  FORMAT! 'O'.  1P9D19.  7/( '  ', IP9D19.  7) ) 

RETURN 
END 

SUBROUTINE  READM! IRMAX.  IR.  IC,  MAT) 

READS  IN  A  MATRIX  BY  ROWS 

IMPLICIT  REALMS  (A-H.K-Z) 

DIMENSION  MAT! IRMAX.  1) 

DO  10  1*1. IN 

READ! 9. 90)  !HAT(I.  J),  J»l,  IC) 

10  CONTINUE 


28 


a^1*1 


n  o  n  nooonnon  n  o 


90  FORMAT <&Dl?.  8) 

RETURN 
END 

SUBR0U1 INS  MULMAT ( IRAMX,  IRA.  ICA. A.  IRBMX,  ICB. B.  IRCMX. C ) 

C  *  A  *  B,  MULTIPLIES  2  MATRICES 

IMPLICIT  REAL *8  (A-H,  K-Z) 

DIMENSION  A(  IRAMX.  1 ) ,  B(  IRBMX.  D.CdRCMX.  1) 

DO  10  I* I . IRA 
DO  10  J* 1. ICB 
C(I.U)  '  0.  ODO 
DO  10  IK* 1.  ICA 

10  C(I.J)  *  C(I.J)  +  Ad.  IK)  #  B ( IK.  J> 

RETURN 
END 

SUBROUT  INE  MATINVdNAMX,  INBMX.  INCMX.  INDMX.  INA.  A.  B.  C.  D.  TV,  IV) 

INVERTS  THE.  MATRIX  A  BY  SOLVING  THE  SET:  A  *  B  -  C  FOR  B. 
WHERE  C  -  A  UNIT  MATRIXi  B  -  AdNV) 

METHOD:  LU  DECOMPOSITION;  REF:  FORSYTHE  t>  MOLER. 

'COMPUTER  SOLUTIONS  OF  LINEAR  ALGEBRAIC  SYSTEMS' 
SUBROUTINES:  DECOMP  &  SOLVE  .  SUPPLIED  BY  DR.  L 
ANDERSON  OF  VP  I 

IMPLICIT  RFAL*8  (A-H.  K-Z > 

DIMENSION  A(  INAMX,  1  >,  B(  INBMX.  D.CdNCMX.  1 ) .  D<  INDMX.  l).TV(l) 
INTEGER  IV(1> 

DECOMPOSE  A  INTO  LU  FACTORS 
CALL  DECOW ( INA.  A.  INAMX.  D.  INDMX.  IV.  TV) 

SOI VE  THE  LINEAR  SE1  A  *  B(1.U>  -  I  FOR  J-l.  INA 
SET  C  10  IDENTITY  HTRX 
DO  10  J-  1 .  INA 
DO  9  I*  1.  INA 
9  C(I.J)  *  O.  ODO 
C(JtJ)  -  1.  ODO 

10  CALL  SSOl  VE ( INA.  D.  INDMX,  C ( 1.  U>.  B ( 1 .  J) , IV) 

C  NOTE:  C  IS  A  MATRIX  FOR  CONVENIENCE 

RETURN 
END 


SUBROUT INE  DECOMP (N.  A,  NA.  UL.  NU.  IPS.  SCALES) 
REAL«8  A(NA. 1 > . UL(NU.  1 > .  SCALES ( 1 > 

REAL*8  ROWNRM.  SIZE.  BIO.  PIVOT.  EM 
INTEOER  IPS(l) 

C  INITIALIZE  IPS.  UL.  AND  SCALES 
DO  9  1*1.  N 
IP8U)  *  I 
ROWNRM  *  0.  ODO 
DO  3  Jd.N 
Utd.J)  *  Ad.  J) 

IF(ROWNRM-DABS<UL( I.  J) ) > 1, 2. 2 

I  ROWNRM  *  DABS(ULd.J)) 

3  CONTINUE 

IF (ROWNRM) 3.  4.3 

3  SCALES ( I )  *  1. /ROWNRM 
00  TO  5 

4  CALL  SINOd) 

SCALES ( I )  *  0.  ODO 

9  CONTINUE 

C  GAUSSIAN  ELIMINATION  WITH  PARTIAL  PIVOTING 
NM1  -  N-l 
DO  17  K'l.NMl 
BIO  -  0.  ODO 
DO  11  I*K,  N 
IP  -  IPSd) 

SIZE  ■  DABS(UL( IP,  K) )*SCALE8( IP ) 

IF(8IZE *BIO) 1 1, 11,  10 

10  BIO  ■  SIZE 
IDXPIV*  1 

II  CONTINUE 
1F(II0> 13.  13,  13 


$ 


I 


v  I 

%  >  P 


CALL  SINGI?) 

CO  TO’  17 

IF  < IOXPIV -K>14,  15,  14 
J*"  IPS  IK) 

IPS(K)  *•  IPSIIDXPIV) 

IPSIIDXPIV)  -  J 
KP  -  IPS(K) 

PIVOT  »  Ul (KP, K) 

KP1  «  K  y  1 
DO  16  I*  KP1 ,  N 

ip  -  ipsid 

EM  -  -ULIIP.KI/PIVOT 
Ul  I  IP,  K >  *  -EH 
DO  16  U*  KP  1 «  N 

UL( IP, J>  »  UL( IP,  J)  ♦  EM*UL!KP.J> 

CONTINUt 
CONTINUf 
KP  -  IPO IN) 

IFIL&.IKP,  N)  >19,  18,  19 
CALL  S1NCU') 

RETURN 

END 

SUBROUTINE  SSOLVEIN,  UL,  NU, B,  X,  IPS ) 

R£AL*8  UL (NU, 1),  B  !  1 ) .  X(l>,  SUM 
INTEGER  1PC(1) 

NP1  ■  N  ►  I 
IP  -  IP3I1) 

X(l)  -  R I  IP  > 

DO  2  I*?,N 

IP  -  ipsid 

IH1  ■  1  -1 

sun  -  o.  opo 

do  l  u>  i,  mi 

sun  •  sun  ►  ul<ip,  j>*xij> 
xm  •  ri  ip  >  -  sun 

IP  -  IPSIN) 

X (N>  *  X(N)/UL( IP,  N> 

DO  4  IBACK  -  2.N 
I  -  NP1  -  (BACK 
IP  •  IP3!t> 

IP1  -  I  f  1 

sun  -  o.  oik) 

DO  3  J*  IP1.N 

sun  -  sun  ►  ul iip,  j>*xij> 

X<1)  »  ( x < I >-8UH) /UL< IP.  I) 

RETURN 

end 

SUBROUTINE  SING ( I WHY) 

DATA  NOUT/6/ 

00  T0!l.?.3>,  I WHY 
WRITEINOUT ,  11 ) 

RETURN 

WRITEINOUT,  12) 

RETURN 

WRITEINOUT,  13) 

RETURN 

FORMAT! '  MATRIX  WITH  ZERO  ROW  IN  DECOMPOSE.  ') 

FORMAT!'  SINGULAR  MATRIX  IN  DECOMPOSE.  ZERO  DIVIDED  IN  SOLVE.  ' > 
FORMAT!'  NO  CONV  IN  IMPROVE,  MATRIX  IS  NEARLY  SINOULAR') 

END 


FILMED 


